#ifndef OBSTACLE_AVOID_FSM_H
#define OBSTACLE_AVOID_FSM_H

#include<ros/ros.h>
#include<Eigen/Eigen>
#include<algorithm>
#include<iostream>
#include<tf/tf.h>
#include<sensor_msgs/PointCloud2.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Int16.h>
#include<std_msgs/Bool.h>
#include<pcl/kdtree/kdtree_flann.h>
#include<pcl_conversions/pcl_conversions.h>
#include<pcl/filters/voxel_grid.h>
#include<pcl/common/transforms.h>
#include<nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/PoseStamped.h>
#include<mavros_msgs/PositionTarget.h>
#include<mavros_msgs/State.h>

namespace obstacle_avoid
{
    class ObstacleAvoidFSM
    {
    private:
        enum OBS_AVOID_STATE
        {
            INIT,
            MANUAL_FLIGHT,
            WARNING_ALERT,
            OPERATOR_LIMIT,
            EMERGENCY_HOVER,
            RETURN_HOME
        };

        //parameters
        double warning_time;
        double limit_time;
        double emergency_time;
        //param for dynamic distance
        double lamda_warning_distance;
        double lamda_limit_distance;
        double lamda_emergency_distance;
        double d_waring;
        double d_limit;
        double d_emergency;
        double t2c; //time 2 collision
        double obs_dis;
        double max_vel;
        double max_acc;
        double hover_vel_threhold;
        double motion_vel_threhold;
        std::deque<double> distance_history_; 
        int filter_window_size_;
        double emergency_hover_duration;
        
        //data
        OBS_AVOID_STATE current_state;
        bool have_odom;
        bool have_point_cloud;
        bool have_imu;
        bool is_motion_init;
        bool is_home_set;
        //bool start_return;
        bool is_emergency;
        bool last_return_flag;
        
        Eigen::Vector3d home_pos;
        Eigen::Vector3d odom_pos;
        Eigen::Vector3d odom_vel;
        Eigen::Vector3d odom_angular;
        Eigen::Vector3d cmd_vel;
        Eigen::Quaterniond odom_orient;
        std_msgs::Bool start_return;
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_data;
        Eigen::Vector3d obstaclePos;
        sensor_msgs::ImuPtr imu_data;
        Eigen::Vector3d last_valid_forward_direction;
        ros::Time emergency_start_time;

        //ROS utils
        ros::NodeHandle node;
        ros::Timer obs_avoid_timer;
        ros::Subscriber home_sub;
        ros::Subscriber odom_sub;
        ros::Subscriber cloud_sub;
        ros::Subscriber imu_sub;
        ros::Subscriber cmd_sub;
        ros::Subscriber traj_cmd_sub;
        ros::Publisher state_pub;
        ros::Publisher cloud_data_pub;
        ros::Publisher return_home_pub;

        //helper functions
        //Dynamic minmum Safe Distance
        double calculateDynamicDmin(double current_vel, double max_acc)
        {
            const double margin = 0.3;
            return (current_vel*current_vel) / (2.0 *  max_acc) + margin;
        }

        //voxel filter
        void downsampleCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data);

        //front direction cloud filter
        void filterThreatRegion(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data);

        //preprocess the point cloud
        void preprocessPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data);

        //find nearest point clound by KD-Tree
        void nearestDistance(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data);

        void temporalFilter();
        
        //calculate time to collision
        //void calculateT2C();

        //get motion direction
        Eigen::Vector3d getmotionDirection();

        //calculate distance
        void calculateDis();

        //Determin whether to hover
        bool isHovering();
        
        //change current state to new state 
        void changeFSMState(OBS_AVOID_STATE new_state, std::string pos_call);
        
        //print current state
        void printFSMState();

        //ROS functions
        void avoidFSMCallback(const ros::TimerEvent &e);
        void homeCallback(const geometry_msgs::PoseStampedConstPtr &msg);
        void odomCallback(const nav_msgs::OdometryConstPtr &msg);
        void cloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg);
        void imuCallback(const sensor_msgs::ImuConstPtr &msg);
        void cmdCallback(const mavros_msgs::PositionTargetConstPtr &msg);
        void cloudPub(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data);
        void statePub();

    public:
        int const getCurrentState() const
        {
            return static_cast<int>(this->current_state);
        }

        void init(ros::NodeHandle &nh);

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
}// namespace obstacle_avoid

#endif